#include <caros/ur_service_interface.h>

#include <caros/common.h>
#include <caros/common_robwork.h>

#include <rw/math/Q.hpp>
#include <rw/math/Transform3D.hpp>

#include <ros/ros.h>

#include <algorithm>

URServiceInterface::URServiceInterface(const ros::NodeHandle& nodehandle)
    : nodehandle_(nodehandle, UR_SERVICE_INTERFACE_SUB_NAMESPACE),
      ur_service_async_spinner_(ASYNC_SPIN_USE_THREAD_FOR_EACH_CORE, &ur_service_cb_queue_)
{
  /* Do nothing for now */
  /* No way to verify that this object is properly configured or just a zombie object, since RAII is not being used */
}

URServiceInterface::~URServiceInterface()
{
  ros::waitForShutdown();
}

bool URServiceInterface::configureInterface()
{
  if (srv_ur_servo_stop_ || srv_ur_servo_q_ || srv_ur_force_mode_start_ || srv_ur_force_mode_update_ ||
      srv_ur_force_mode_stop_ || srv_ur_move_lin_ || srv_ur_move_ptp_ || srv_ur_move_ptp_path_ || srv_ur_move_vel_q_ ||
      srv_ur_move_vel_t_ || srv_ur_set_payload_ || srv_ur_set_io_ || ur_state_publisher_)
  {
    ROS_WARN_STREAM(
        "Reinitialising one or more URServiceInterface services. If this is not fully intended then this should be "
        "considered a bug!");
  }

  ur_state_publisher_ =
      nodehandle_.advertise<caros_control_msgs::RobotState>("robot_state", UR_STATE_PUBLISHER_QUEUE_SIZE);
  ROS_ERROR_STREAM_COND(!ur_state_publisher_, "The RobotState publisher is empty!");

  srv_ur_servo_stop_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceEmpty>(
      "move_servo_stop", boost::bind(&URServiceInterface::urServoStopHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_servo_stop_ = nodehandle_.advertiseService(srv_ur_servo_stop_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_servo_stop_, "The move_servo_stop service is empty!");

  srv_ur_servo_q_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceServoQ>(
      "move_servo_q", boost::bind(&URServiceInterface::urServoQHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_servo_q_ = nodehandle_.advertiseService(srv_ur_servo_q_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_servo_q_, "The move_servo_q service is empty!");

  srv_ur_force_mode_start_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceForceModeStart>(
      "force_mode_start", boost::bind(&URServiceInterface::urForceModeStartHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_force_mode_start_ = nodehandle_.advertiseService(srv_ur_force_mode_start_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_force_mode_start_, "The force_mode_start service is empty!");

  srv_ur_force_mode_update_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceForceModeUpdate>(
      "force_mode_update", boost::bind(&URServiceInterface::urForceModeUpdateHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_force_mode_update_ = nodehandle_.advertiseService(srv_ur_force_mode_update_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_force_mode_update_, "The force_mode_update service is empty!");

  srv_ur_force_mode_stop_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceForceModeStop>(
      "force_mode_stop", boost::bind(&URServiceInterface::urForceModeStopHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_force_mode_stop_ = nodehandle_.advertiseService(srv_ur_force_mode_stop_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_force_mode_stop_, "The force_mode_stop service is empty!");

  srv_ur_move_lin_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceMoveLin>(
      "move_lin", boost::bind(&URServiceInterface::urMoveLinHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_move_lin_ = nodehandle_.advertiseService(srv_ur_move_lin_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_move_lin_, "The move_lin service is empty!");

  srv_ur_move_ptp_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceMovePtp>(
      "move_ptp", boost::bind(&URServiceInterface::urMovePtpHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_move_ptp_ = nodehandle_.advertiseService(srv_ur_move_ptp_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_move_ptp_, "The move_ptp service is empty!");

  srv_ur_move_ptp_path_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceMovePtpPath>(
      "move_ptp_path", boost::bind(&URServiceInterface::urMovePtpPathHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_move_ptp_path_ = nodehandle_.advertiseService(srv_ur_move_ptp_path_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_move_ptp_path_, "The move_ptp_path service is empty!");

  srv_ur_move_vel_q_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceMoveVelQ>(
      "move_vel_q", boost::bind(&URServiceInterface::urMoveVelQHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_move_vel_q_ = nodehandle_.advertiseService(srv_ur_move_vel_q_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_move_vel_q_, "The move_vel_q service is empty!");

  srv_ur_move_vel_t_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceMoveVelT>(
      "move_vel_t", boost::bind(&URServiceInterface::urMoveVelTHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_move_vel_t_ = nodehandle_.advertiseService(srv_ur_move_vel_t_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_move_vel_t_, "The move_vel_t service is empty!");

  srv_ur_set_payload_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServicePayload>(
      "set_payload", boost::bind(&URServiceInterface::urSetPayloadHandle, this, _1, _2), ros::VoidPtr(),
      &ur_service_cb_queue_);

  srv_ur_set_payload_ = nodehandle_.advertiseService(srv_ur_set_payload_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_set_payload_, "The set_payload service is empty!");

  srv_ur_set_io_ops_ = ros::AdvertiseServiceOptions::create<caros_universalrobot::UrServiceSetIO>(
      "set_io", boost::bind(&URServiceInterface::urSetIOHandle, this, _1, _2), ros::VoidPtr(), &ur_service_cb_queue_);

  srv_ur_set_io_ = nodehandle_.advertiseService(srv_ur_set_io_ops_);
  ROS_ERROR_STREAM_COND(!srv_ur_set_io_, "The set_io service is empty!");

  ur_service_async_spinner_.start();

  if (srv_ur_servo_stop_ && srv_ur_servo_q_ && srv_ur_force_mode_start_ && srv_ur_force_mode_update_ &&
      srv_ur_force_mode_stop_ && srv_ur_move_lin_ && srv_ur_move_ptp_ && srv_ur_move_ptp_path_ && srv_ur_move_vel_q_ &&
      srv_ur_move_vel_t_ && srv_ur_set_payload_ && ur_state_publisher_)
  {
    /* Everything seems to have been properly initialised */
    ROS_DEBUG_STREAM("All URServiceInterface publishers and services appear to have been properly initialised");
  }
  else
  {
    ROS_ERROR_STREAM(
        "The URService could not be properly initialised - one or more ros services may not be up and running or "
        "working as intended!");
    return false;
  }

  return true;
}

void URServiceInterface::publishState(const caros_control_msgs::RobotState& state)
{
  ur_state_publisher_.publish(state);
}

bool URServiceInterface::urMoveLinHandle(caros_universalrobot::UrServiceMoveLin::Request& request,
                                         caros_universalrobot::UrServiceMoveLin::Response& response)
{
  rw::math::Transform3D<> target = caros::toRw(request.target);
  response.success = urMoveLin(target, request.speed, request.acceleration);

  return true;
}

bool URServiceInterface::urMovePtpHandle(caros_universalrobot::UrServiceMovePtp::Request& request,
                                         caros_universalrobot::UrServiceMovePtp::Response& response)
{
  rw::math::Q target = caros::toRw(request.target);
  response.success = urMovePtp(target, request.speed, request.acceleration);

  return true;
}

bool URServiceInterface::urMovePtpPathHandle(caros_universalrobot::UrServiceMovePtpPath::Request& request,
                                             caros_universalrobot::UrServiceMovePtpPath::Response& response)
{
  rw::trajectory::QPath q_path;
  for (unsigned int i = 0; i < request.targets.size(); i++)
  {
    rw::math::Q target = caros::toRw(request.targets[i]);
    double speed = request.speeds[i];
    double acceleration = request.accelerations[i];
    double blend = request.blends[i];
    rw::math::Q q(9, target[0], target[1], target[2], target[3], target[4], target[5], speed, acceleration, blend);
    q_path.push_back(q);
  }

  response.success = urMovePtpPath(q_path);

  return true;
}

bool URServiceInterface::urMoveVelQHandle(caros_universalrobot::UrServiceMoveVelQ::Request& request,
                                          caros_universalrobot::UrServiceMoveVelQ::Response& response)
{
  rw::math::Q vel = caros::toRw(request.vel);
  response.success = urMoveVelQ(vel, request.acceleration, request.time);

  return true;
}

bool URServiceInterface::urMoveVelTHandle(caros_universalrobot::UrServiceMoveVelT::Request& request,
                                          caros_universalrobot::UrServiceMoveVelT::Response& response)
{
  rw::math::VelocityScrew6D<> vel = caros::toRw(request.vel);
  response.success = urMoveVelT(vel, request.acceleration, request.time);

  return true;
}

bool URServiceInterface::urMoveStopHandle(caros_universalrobot::UrServiceEmpty::Request& request,
                                          caros_universalrobot::UrServiceEmpty::Response& response)
{
  response.success = urMoveStop();

  return true;
}

bool URServiceInterface::urServoStopHandle(caros_universalrobot::UrServiceEmpty::Request& request,
                                           caros_universalrobot::UrServiceEmpty::Response& response)
{
  response.success = urServoStop();
  return true;
}

bool URServiceInterface::urServoQHandle(caros_universalrobot::UrServiceServoQ::Request& request,
                                        caros_universalrobot::UrServiceServoQ::Response& response)
{
  rw::math::Q target = caros::toRw(request.target);
  response.success = urServoQ(target, request.time, request.lookahead_time, request.gain);
  return true;
}

bool URServiceInterface::urForceModeStartHandle(caros_universalrobot::UrServiceForceModeStart::Request& request,
                                                caros_universalrobot::UrServiceForceModeStart::Response& response)
{
  rw::math::Transform3D<> ref_t_offset = caros::toRw(request.base2forceFrame);
  rw::math::Wrench6D<> wrench_target;
  wrench_target(0) = request.wrench.force.x;
  wrench_target(1) = request.wrench.force.y;
  wrench_target(2) = request.wrench.force.z;

  wrench_target(3) = request.wrench.torque.x;
  wrench_target(4) = request.wrench.torque.y;
  wrench_target(5) = request.wrench.torque.z;

  std::size_t index;
  rw::math::Q selection(request.selection.size());
  index = 0;
  for (const auto item : request.selection)
  {
    selection(index++) = static_cast<double>(item);
  }

  rw::math::Q limits(request.limits.size());
  index = 0;
  for (const auto item : request.limits)
  {
    limits(index++) = static_cast<double>(item);
  }

  response.success = urForceModeStart(ref_t_offset, selection, wrench_target, 2, limits);
  return true;
}

bool URServiceInterface::urForceModeUpdateHandle(caros_universalrobot::UrServiceForceModeUpdate::Request& request,
                                                 caros_universalrobot::UrServiceForceModeUpdate::Response& response)
{
  rw::math::Wrench6D<> wrench_target;
  wrench_target(0) = request.wrench.force.x;
  wrench_target(1) = request.wrench.force.y;
  wrench_target(2) = request.wrench.force.z;

  wrench_target(3) = request.wrench.torque.x;
  wrench_target(4) = request.wrench.torque.y;
  wrench_target(5) = request.wrench.torque.z;

  response.success = urForceModeUpdate(wrench_target);
  return true;
}

bool URServiceInterface::urForceModeStopHandle(caros_universalrobot::UrServiceForceModeStop::Request& request,
                                               caros_universalrobot::UrServiceForceModeStop::Response& response)
{
  response.success = urForceModeStop();
  return true;
}

bool URServiceInterface::urSetPayloadHandle(caros_universalrobot::UrServicePayload::Request& request,
                                            caros_universalrobot::UrServicePayload::Response& response)
{
  double mass = request.mass;
  rw::math::Vector3D<> center_of_mass;
  center_of_mass[0] = request.com.at(0);
  center_of_mass[1] = request.com.at(1);
  center_of_mass[2] = request.com.at(2);

  response.success = urSetPayload(mass, center_of_mass);
  return true;
}

bool URServiceInterface::urSetIOHandle(caros_universalrobot::UrServiceSetIO::Request& request,
                                       caros_universalrobot::UrServiceSetIO::Response& response)
{
  int pin = request.pin;
  bool value = request.value;

  response.success = urSetIO(pin, value);
  return true;
}
